//
// Created by zwh on 2022/4/12.
//

#ifndef BFRAME_POINT_NEW_H
#define BFRAME_POINT_NEW_H

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

#include <utility>

//自定义点的类型
struct PointXYZIDRC              //定义点类型结构
{
    PCL_ADD_POINT4D;                // 该点类型有4个元素
    PCL_ADD_INTENSITY;
    float dis;//深度
    uint16_t col;//列數
    uint16_t row;//行數(ring)
    //double timestamp;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW// 确保new操作符对齐操作
}EIGEN_ALIGN16;// 强制SSE对齐

POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIDRC,// 注册点类型宏
                                  (float,x,x)
                                          (float,y,y)
                                          (float,z,z)
                                          (float, intensity, intensity)
                                          (float, dis, dis)
                                          (uint16_t, col, col)
                                          (uint16_t, row, row)
                                          //(double, timestamp, timestamp)
)

typedef PointXYZIDRC Pointt;
typedef pcl::PointCloud<PointXYZIDRC> Cloud;
typedef pcl::PointCloud<PointXYZIDRC>::Ptr CloudPtr;
typedef pcl::PointCloud<PointXYZIDRC>::ConstPtr CloudConstPtr;

//typedef pcl::PointXYZI Pointt;
//typedef pcl::PointCloud<pcl::PointXYZI> Cloud;
//typedef pcl::PointCloud<pcl::PointXYZI>::Ptr CloudPtr;
//typedef pcl::PointCloud<pcl::PointXYZI>::ConstPtr CloudConstPtr;

template <typename T>
class CloudSet : public pcl::PointCloud<T>{
private:
    size_t frame_len;
public:
    void serilize(std::string &buffer) {
        frame_len = this->header.frame_id.size();
        buffer.resize(sizeof(this->header.stamp)+sizeof(this->header.seq)+sizeof(frame_len)+frame_len+sizeof(T)* this->points.size());
        memcpy((char *) buffer.c_str(), &this->header.stamp, sizeof(this->header.stamp));
        memcpy((char *) buffer.c_str()+sizeof(this->header.stamp), &this->header.seq, sizeof(this->header.seq));
        memcpy((char *) buffer.c_str()+sizeof(this->header.stamp)+sizeof(this->header.seq), &frame_len, sizeof(frame_len));
        memcpy((char *) buffer.c_str()+sizeof(this->header.stamp)+sizeof(this->header.seq)+sizeof(frame_len), this->header.frame_id.c_str(), frame_len);
        memcpy((char *) buffer.c_str()+sizeof(this->header.stamp)+sizeof(this->header.seq)+sizeof(frame_len)+frame_len, this->points.data(), sizeof(T)* this->points.size());
    }

    void parse(const std::string &buffer) {
        memcpy(&this->header.stamp,buffer.c_str(),sizeof(this->header.stamp));
        memcpy(&this->header.seq,buffer.c_str()+sizeof(this->header.stamp),sizeof(this->header.seq));
        memcpy(&frame_len,buffer.c_str()+sizeof(this->header.stamp)+sizeof(this->header.seq),sizeof(frame_len));
        this->header.frame_id.resize(frame_len);
        memcpy((char *)this->header.frame_id.c_str(),buffer.c_str()+sizeof(this->header.stamp)+sizeof(this->header.seq)+sizeof(frame_len),frame_len);
        this->points.resize((buffer.size()-sizeof(this->header.stamp)-sizeof(this->header.seq)-sizeof(frame_len)-frame_len) / sizeof(T));
        memcpy(this->points.data(), buffer.c_str()+sizeof(this->header.stamp)+sizeof(this->header.seq)+sizeof(frame_len)+frame_len,
               buffer.size()-sizeof(this->header.stamp)-sizeof(this->header.seq)-sizeof(frame_len)-frame_len);
    }
};

struct Pose{
    pcl::PCLHeader header;
    Eigen::Quaterniond q;
    Eigen::Vector3d t;
};

#endif //BFRAME_POINT_NEW_H
